
#include "filter.h"
#include <math.h>

#define PI				(3.14159265358979232846)
#define IzeroEPSILON	1E-21	


float Izero(float x)
{
   float sum, u, halfx, temp;
   int n;

   n = 1;
   sum = u = 1.0F;
   halfx = x/2.0F;
   do {
      temp = halfx/(float)n;
      n += 1;
      temp *= temp;
      u *= temp;
      sum += u;
   } while (u >= IzeroEPSILON*sum);
   return sum;
}

int LpFilter(float coeff[], float cutoff, int N, float gain)
{
	int i;
	if (N<=0 || cutoff<=0 || cutoff>0.5)
		return 0;
	coeff[0] = 2.0F * cutoff * gain;
	float temp = 0;
	for (i=1; i<N; ++i)
	{
		temp = (float)(i * PI);
		coeff[i] = (float)sin(2.0*temp*cutoff) / temp;
	}
	
	float temp1 = 0;
	float IBeta = 1.0F/Izero(KAISER_BETA);
    float inm1 = 1.0F/((float)(N-1));
    for (i=1; i<N; ++i) {
		temp = (float)i * inm1;
		temp1 = 1.0F - temp*temp;
		temp1 = (temp1<0? 0: temp1); 
		coeff[i] *= (Izero((float)(KAISER_BETA*sqrt(temp1))) * IBeta * gain);
	}	
	return 1;
}

//coeffp must be at least ROUND_UP(2N-1, M) in size
int PolyPhaseDecomposition (float coeff[], float coeffp[], int N, int M)
{
	int i, j;
	if (N <= 0 || M <= 0)
		return 0;
	float* dst = coeffp;
	int sub = (int)(ceil(float(2*N-1)/float(M)));
	for (i=N-M; i<N; ++i)
	{
		for (j=i; j>=0; j-=M)
			*dst++ = coeff[j];
		for (j=-j; j<N; j+=M)
			*dst++ = coeff[j];
		if ((dst-coeffp) % sub)
			*dst++ = 0;
	}
	return 1;
}
